import math
import time

from Controller.HttpUtil import *

class Marker:
    def __init__(self, floor, name, x, y, theta):
        self.floor = floor
        self.name = name
        self.pose = {
            'x': x,
            'y': y,
            'theta': theta
        }

    def __repr__(self):
        return f"Marker(floor='{self.floor}', name='{self.name}', x={self.pose['x']}, y={self.pose['y']}, theta={self.pose['theta']})"


class Water:
    def __init__(self, ip_address, map_code=None):
        self.base_url = ip_address
        if map_code:
            self.map_code = map_code
        self.httpTool = HttpTool(self.base_url)

    def _retry_request(self, method, url, params=None, max_retries=3):
        """通用重试逻辑"""
        for attempt in range(max_retries + 1):
            response = method(url, params=params)
            if response.get('status') == 'OK':
                return response
            else:
                print(f"Attempt {attempt + 1}/{max_retries}")
        else:
            print(f"Attempt {attempt + 1}/{max_retries}: HTTP Error {response.status_code}")
        if attempt < max_retries:
            time.sleep(1)  # 等待1秒后重试
        raise Exception(f"Failed after {max_retries} attempts")

    def get_robot_code(self):
        """获取机器人信息"""
        url = f"/api/robot_info"
        response_data = self._retry_request(self.httpTool.get, url)
        return response_data["results"]['product_id']

    def get_robot_status(self):
        """获取机器人当前全局状态"""
        url = f"/api/robot_status"
        response_data = self._retry_request(self.httpTool.get, url)
        if response_data["results"]["move_status"] == "idle" or response_data["results"]["running_status"] == "idle":
            return response_data["results"]
        else:
            return None

    def move_to_marker(self, marker_name):
        """导航到指定的点位"""
        url = f"/api/move"
        params = {
            "marker": marker_name
        }
        response_data = self._retry_request(self.httpTool.get, url, params=params)
        return response_data.get('task_id', 'Task ID not provided')

    def move_to_pose(self, x, y, theta, floor):
        """导航到指定坐标朝向"""
        url = f"/api/move?location={x},{y},{theta}&floor={floor}"
        response_data = self._retry_request(self.httpTool.get, url)
        return response_data

    def get_move_status(self):
        """检查底盘导航状态"""
        url = f"/api/robot_status"
        response_data = self._retry_request(self.httpTool.get, url)
        status_info = response_data.get('results', {})
        return {
            'move_target': status_info.get('move_target'),
            'move_status': status_info.get('move_status'),
            'running_status': status_info.get('running_status')
        }

    def cancel_move(self):
        """取消当前导航"""
        url = f"/api/move/cancel"
        response_data = self._retry_request(self.httpTool.get, url)
        return response_data

    def get_cur_map(self):
        """取消当前导航"""
        url = f"/api/map/get_current_map"
        response_data = self._retry_request(self.httpTool.get, url)
        return response_data

    def get_markers(self, name=None):
        """获取所有点位信息"""
        url = f"/api/markers/query_list"
        response_data = self._retry_request(self.httpTool.get, url)
        markers_data = response_data.get('results', {})
        markers = []
        for marker in markers_data.items():
            floor = marker[1]['floor']
            pose = marker[1]['pose']
            point_name = marker[1]['marker_name']
            position = pose['position']
            orientation = pose['orientation']
            x = position['x']
            y = position['y']
            theta = 2 * math.atan2(orientation['z'], orientation['w'])
            if theta > math.pi:
                theta -= 2 * math.pi
            elif theta < -math.pi:
                theta += 2 * math.pi

            # 如果name不是None，只添加匹配的点位
            if name is None or point_name == name:
                markers.append(Marker(floor, point_name, x, y, theta))

        return markers


# 使用示例
if __name__ == "__main__":
    water = Water("192.168.10.10:9001", None)
    try:
        # 获取机器人状态
        status = water.get_robot_status()
        print("Robot Status:", status)
        # 导航到指定点位
        task_id = water.move_to_pose(floor='3', x=2.12, y=0.63, theta=-0.30074285601426887)
        # print(f"Navigation task started with task ID: {task_id}")

        # 获取所有点位
        # markers = water.cancel_move()
        # print("Markers:", markers)
    except Exception as e:
        print(str(e))